#ifndef MODEL_ROBOT_MODEL_H
#define MODEL_ROBOT_MODEL_H

#include "basic_data_structure.hpp"

using namespace Eigen;
namespace ZORAL
{
    class RobotModel
    {
    private:
        std::string robot_name_;
        int dof_{};
        std::vector<Twist> S_;
        std::vector<HomoTrans> M_;
        std::vector<SpaceInertia> G_;
        std::vector<std::vector<double>> joint_limits_;
        Vector3d grav_{0.0, 0.0, -9.81};

    public:
        explicit RobotModel() = default;
        RobotModel(const RobotModel &rm) : dof_(rm.dof_), S_(rm.S_), M_(rm.M_)
        {
        }
        explicit RobotModel(const std::string &robot_name, const std::string &package_path);
        std::string getRobotName()
        {
            return robot_name_;
        }
        int getRobotDOF() const
        {
            return dof_;
        }
        Twist getRobotAxisTwist(const int &i)
        {
            return S_[i];
        }
        HomoTrans getRobotPartialPose(const int &i)
        {
            return M_[i];
        }
        HomoTrans getRobotEndPose()
        {
            return M_[M_.size() - 1];
        }
        Vector3d getGravityVec()
        {
            return grav_;
        }
        void setGravityVec(const Vector3d &grav)
        {
            grav_ = grav;
        }
        SpaceInertia getSpaceInertia(const int &i)
        {
            return G_[i];
        }

        std::vector<std::vector<double>> getJointLimits()
        {
            return joint_limits_;
        }
    };
} // namespace ZORAL
#endif // MODEL_ROBOT_MODEL_H
